#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Hubs,  S4, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S4,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     frontLeft,     tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rearLeft,      tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     arm,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_1,     rearRight,     tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S4_C1_2,     frontRight,    tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S4_C2_1,     lift,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_2,     motorK,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    guiderDrop,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    leftHook,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    rightHook,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    pivot,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    spinner,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include <JoystickDriver.c>

int driveEncoderCycle = 1120;
float manualDriveScale = (0.7874 * 0.7777);
int pivotTarget = 1000;
const int pivotIncrement = 1;

void setPivot(float pivotSetting)
{
	servo [pivot] = 255 * pivotSetting;
}

int resetDriveEncoders()
{
	nMotorEncoder [frontLeft] = 0;
	nMotorEncoder [frontRight] = 0;
	nMotorEncoder [rearLeft] = 0;
	nMotorEncoder [rearRight] = 0;
	return 1;
}

void initializeRobot()
{
	bDisplayDiagnostics = false;
	nxtDisplayCenteredTextLine(3, "Initializaing");
	nxtDisplayCenteredTextLine(4, "servos...");
	servo [pivot] = 0;
	servo [spinner] = 128;
	servo [guiderDrop] = 255;
	servo [leftHook] = 22;
	servo [rightHook] = 209;
	wait1Msec(1000);
	eraseDisplay();
	if (resetDriveEncoders() == 1)
	{
		nxtDisplayCenteredTextLine(3, "Encoders");
		nxtDisplayCenteredTextLine(4, "successfully");
		nxtDisplayCenteredTextLine(5, "reset.");
		wait1Msec(1000);
	}
	eraseDisplay();
	bDisplayDiagnostics = true;
}

task main()
{
	initializeRobot();

	int x1, y1, x2;
	int threshold = 8;
	int liftPrecisionFactor = 0;
	//int pivotSet = 0;

	waitForStart();

	/*
	Encoder demo
	while (nMotorEncoder [frontLeft] < driveEncoderCycle)
	{
	motor [frontLeft] = 12 * autoDriveScale;
	}
	motor [frontLeft] = 0 * autoDriveScale;
	*/

	//run loop
	while (1)
	{
		//joystick refresh
		getJoystickSettings(joystick);

		//lift precision factor - manual control of value
		if (joy2Btn(1) == 1) {
			liftPrecisionFactor += 5;
			while (joy2Btn(1) != 0)
			{
			}
		}
		else if (joy2Btn(3) == 1) {
			liftPrecisionFactor -= 5;
			while (joy2Btn(3) != 0)
			{
			}
		}

		//lift precision factor - automatic control of bounds
		if (liftPrecisionFactor <= 0) {
			liftPrecisionFactor = 0;
		}
		else if (liftPrecisionFactor >= 100) {
			liftPrecisionFactor = 95;
		}

		//drive code
	x1 = (abs(joystick.joy1_x1) > threshold) ? ((joystick.joy1_x1 * manualDriveScale) * 0.75) : 0;
	y1 = (abs(joystick.joy1_y1) > threshold) ? ((joystick.joy1_y1 * manualDriveScale) * 0.75) : 0;
	x2 = (abs(joystick.joy1_x2) > threshold) ? ((joystick.joy1_x2 * manualDriveScale) * 0.75) : 0;

		motor [frontLeft] = y1 + x2 + x1;
		motor [frontRight] = y1 - x2 - x1;
		motor [rearLeft] = y1 + x2 - x1;
		motor [rearRight] = y1 - x2 + x1;
		//pivot control
		if (joystick.joy2_y1 > 110 || joystick.joy2_y2 > 110)
			if (pivotTarget < 1000)
		{
			pivotTarget += pivotIncrement;
			setPivot(pivotTarget/1000.0);
		}
		if (joystick.joy2_y1 < -110 || joystick.joy2_y2 < -110)
			if (pivotTarget > 0)
		{
			pivotTarget -= pivotIncrement;
			setPivot(pivotTarget/1000.0);
		}
		//arm control
		if (joy2Btn(5) == 1) {
			motor [lift] = 100 - liftPrecisionFactor;
		}
		else if (joy2Btn(7) == 1) {
			motor [lift] = -100 + liftPrecisionFactor;
		}
		else {
			motor [lift] = 0;
		}
		if (joy2Btn(6) == 1)
		{
			motor [arm] = -50;
		}
		else if (joy2Btn(8) == 1)
		{
			motor [arm] = 50;
		}
		else
		{
			motor [arm] = 0;
		}
		if (joystick.joy2_TopHat == 6)
		{
			servo [leftHook] = 22;
			servo [rightHook] = 209;
		}
		else if (joystick.joy2_TopHat == 2)
		{
			servo [leftHook] = 255;
			servo [rightHook] = 0;
		}
		if (joystick.joy2_TopHat == 4)
		{
			servo [spinner] = 255;
		}
		else if (joystick.joy2_TopHat == 0)
		{
			servo [spinner] = 0;
		}
		else if (joy2Btn(2) == 1 || joy2Btn(4) == 1)
		{
			servo [spinner] = 128;
		}
		if (joy1Btn(10) && joy2Btn(10))
		{
			servo [guiderDrop] = 0;
		}
		//if (joy2Btn(4) == 1)
		//{
		//	if (pivotSet == 0)
		//	{
		//		servo [pivot] = 255;
		//		pivotSet = 1;
		//	}
		//	else if (pivotSet == 1)
		//	{
		//		servo [pivot] = 0;
		//		pivotSet = 0;
		//	}
		//}
	}
}
